﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace RayTracing
{
    public class P3
    {
        public float x, y, z;                           // x, y, and z cordnates for a point in 3-space.
                                                        // x = left,right.
                                                        // y = forward, back.
                                                        // z = up, down.
                                                        // why? Because.

        public P3(float x, float y, float z)            // Basic constructor takes the float x,y, and z.
        {
            this.x = x;
            this.y = y;
            this.z = z;
        }

        public P3(P3 p)                                 // creates a new p3 from an old one.
        {
            this.x = p.x;
            this.y = p.y;
            this.z = p.z;
        }

        public static P3 add(P3 p, P3 p2)               // static method to add two P3's component wise.
        {
            return new P3(p.x + p2.x, p.y + p2.y, p.z + p2.z);
        }

        public P3 add(P3 p)                             // adds two P3's calls static method add.
        {
            return P3.add(this, p);
        }

        public static P3 sub(P3 p, P3 p2)               // static method to subtract two P3's component wise.
        {
            return new P3(p.x - p2.x, p.y - p2.y, p.z - p2.z);
        }

        public P3 sub(P3 p)                             // subtracts two P3's calls static method sub.
        {
            return P3.sub(this, p);
        }

        public static P3 mul(P3 p, P3 p2)              // static method to multiply two P3's component wise.
        {
            return new P3(p.x*p2.x, p.y*p2.y, p.z*p2.z);
        }

        public P3 mul(P3 p)                            // multiplies two P3's calls static method mul.
        {
            return P3.mul(this, p);
        }

        public static P3 scale(P3 p, float scaler)     // static method to scale a P3 by a value.
        {
            return new P3(p.x * scaler, p.y * scaler, p.z * scaler);
        }

        public P3 scale(float s)                        // scales a P3 calls static method scale.
        {
            return P3.scale(this, s);
        }

        public static float norm(P3 p)                  // norms a point (Dark Magic).
        {
            return ((float) Math.Sqrt((p.x * p.x + p.y * p.y + p.z * p.z)));
        }

        public static P3 normalize(P3 p)                // nomalizes a point(Deep Magic).
        {
            if (norm(p) == 0)
            {
                return new P3(0, 0, 0);
            }
            else
            {
                return scale(p, (float)(1.0 / (norm(p) * 1.0)));
            }
        }

        public static float dot(P3 p, P3 p2)            // dot product of two points.(Deep Dark Magic).
        {
            return ((p.x * p2.x) + (p.y * p2.y) + (p.z * p2.z));
        }

        public static P3 cross(P3 p,P3 p2){             // cross product of two points. (Extremely Deep Dark Magic). 
            float x = (p.y * p2.z) - (p.z * p2.y);
            float y = (p.z * p2.x) - (p.x * p2.z);
            float z = (p.x * p2.y) - (p.y * p2.x);
        return new P3(x,y,z);
    }

        public static P3 fromSpherical(float heading, float pitch)      // because we need it. what it does no one knows... yet.
        {
            float x = (float) (Math.Sin(heading) * Math.Cos(pitch));
            float y = (float) (Math.Sin(heading) * Math.Sin(pitch));
            float z = (float) Math.Cos(heading);
            return new P3(x, y, z);
        }

        public static float getAxis(P3 x, int a)
        {
            if (a == 0) return x.x;
            if (a == 1) return x.y;
            return x.z;
        }
        public static float[] hprGen(P3 hpr)
        {
            float h = hpr.x;
            float p = hpr.y;
            float r = hpr.z;

            float[] Rx = {    1, 0, 0, 0, 
                              0, (float)Math.Cos(r), (float)-Math.Sin(r),0,
                              0, (float)Math.Sin(r),(float)Math.Cos(r),0,
                              0,0,0,1};

            float[] Ry = {  (float)Math.Cos(p), 0, (float)-Math.Sin(p), 0,
                             0, 1, 0 ,0,
                            (float)Math.Sin(p),0,(float)Math.Cos(p),0,
                             0,0,0,1};

            float[] Rz = {  (float)Math.Cos(h), (float)-Math.Sin(h),0,0,
                            (float)Math.Sin(h),(float)Math.Cos(h),0,0,
                             0, 0, 1,0,
                             0,0,0,1};

            float[] Rp = matrixMult(Rx, Ry);
            float[] Rf = matrixMult(Rp, Rz);
            return Rx;
        }

         public static float[] hprGen(P3 hpr,bool un)
        {
            float h = hpr.x;
            float p = hpr.y;
            float r = hpr.z;
            if (un)
            {
                h = -h;
                p = -p;
                r = -r;

            }

            float[] Rx = {    1, 0, 0, 0, 
                              0, (float)Math.Cos(r), (float)-Math.Sin(r),0,
                              0, (float)Math.Sin(r),(float)Math.Cos(r),0,
                              0,0,0,1};

            float[] Ry = {  (float)Math.Cos(p), 0, (float)-Math.Sin(p), 0,
                             0, 1, 0 ,0,
                            (float)Math.Sin(p),0,(float)Math.Cos(p),0,
                             0,0,0,1};

            float[] Rz = {  (float)Math.Cos(h), (float)-Math.Sin(h),0,0,
                            (float)Math.Sin(h),(float)Math.Cos(h),0,0,
                             0, 0, 1,0,
                             0,0,0,1};
           // return Rx;
             
             if (un)
            {
                float[] Rp1 = matrixMult(Rz, Ry);
                float[] Rf1 = matrixMult(Rp1, Rx);
                return Rf1;

            }

            float[] Rp = matrixMult(Rx, Ry);
            float[] Rf = matrixMult(Rp, Rz);
            return Rf;
        }

         public static float[] hprGen(float h,float p,float r, bool un)
         {
             if (un)
             {
                 h = -h;
                 p = -p;
                 r = -r;

             }

             float[] Rx = {    1, 0, 0, 0, 
                              0, (float)Math.Cos(r), (float)-Math.Sin(r),0,
                              0, (float)Math.Sin(r),(float)Math.Cos(r),0,
                              0,0,0,1};

             float[] Ry = {  (float)Math.Cos(p), 0, (float)-Math.Sin(p), 0,
                             0, 1, 0 ,0,
                            (float)Math.Sin(p),0,(float)Math.Cos(p),0,
                             0,0,0,1};

             float[] Rz = {  (float)Math.Cos(h), (float)-Math.Sin(h),0,0,
                            (float)Math.Sin(h),(float)Math.Cos(h),0,0,
                             0, 0, 1,0,
                             0,0,0,1};
             // return Rx;

             if (un)
             {
                 float[] Rp1 = matrixMult(Rz, Ry);
                 float[] Rf1 = matrixMult(Rp1, Rx);
                 return Rf1;

             }

             float[] Rp = matrixMult(Rx, Ry);
             float[] Rf = matrixMult(Rp, Rz);
             return Rf;
         }

        public static float[] scaleGen(P3 s)
        {
            float[] scalemat = 
                   {    s.x,0,0,0,
                        0,s.y,0,0,
                        0,0,s.z,0,
                        0,0,0,1};

            return scalemat;
        }

        public static float[] moveGen(P3 m)
        {
            float[] movemat = 
                   {    1,0,0,0,
                        0,1,0,0,
                        0,0,1,0,
                        m.x,m.y,m.z,1};

            return movemat;
        }

        public static P3 multP3Matrix(P3 v, float[] hpr)
        {

            float newx = v.x * hpr[0] + v.y * hpr[4] + v.z * hpr[8] + hpr[12];
            float newy = v.x * hpr[1] + v.y * hpr[5] + v.z * hpr[9] + hpr[13];
            float newz = v.x * hpr[2] + v.y * hpr[6] + v.z * hpr[10] + hpr[14];


            return new P3(newx,newy,newz);

        }

        public static P3 multDirMatrix(P3 v, float[] hpr)
        {

            float newx = v.x * hpr[0] + v.y * hpr[4] + v.z * hpr[8];
            float newy = v.x * hpr[1] + v.y * hpr[5] + v.z * hpr[9];
            float newz = v.x * hpr[2] + v.y * hpr[6] + v.z * hpr[10];


            return new P3(newx, newy, newz);

        }
        public static float[] matrixMult(float[] a, float[] b)
        {
            float[] c;
            
            {
                c = new float[16];
                for (int i = 0; i < 4; i++)
                {
                    for (int j = 0; j < 4; j++)
                    {
                        c[4*j+i] = 0;
                        for (int k = 0; k < 4; k++)
                            c[4 * j + i] = c[4 * j + i] + a[4 * k+ i] * b[4 * j+ k];
                    }
                }
                return c;
            }
         

        }

        public static float[] iMatrix()
        {
            float[] mat ={  1,0,0,0,
                            0,1,0,0,
                            0,0,1,0,
                            0,0,0,1};
            return mat;

        }
        

    }
}


